/**
 * @file adrc.c
 * @brief 
 * 
 * @author 1024Tech (店铺链接：https://1024tech.taobao.com/)
 * @version 1.0
 * @date 2023-06-08
 * 
 * @copyright Copyright (c) 2023  OpenSir Robot Tech.co, Ltd
 * 
 * @par 修改日志:
 * <table>
 * <tr><th>Date       <th>Version <th>Author  <th>Description
 * <tr><td>2023-05-08 <td>1.0     <td>Mr.lin     <td>Release
 * </table>
 */
#include "adrc.h"
#include "math.h"
#include "stdlib.h"
#include "stdint.h"


LADRC_CTRL ladrc_vel;
LADRC_CTRL ladrc_gyro;

/**
  * @brief   ladrc参数值（5组数据，作为参考）
  */
const float ladrc_parameter[5][5]=
{
	{0.005,20,100,400,0.5},
	{0.001,20,33,133,8},
	{0.005,100,20,80,0.5},
	{0.005,100,14,57,0.5},
	{0.005,100,50,10,1}
};

/**
  * @brief   ladrc参数初始化
  * @param   *ladrc：结构体
  * @param   label：对应的参数组号
  */
void ladrc_para_init(LADRC_CTRL *ladrc,uint8_t label)
{
	ladrc->h = ladrc_parameter[label][0]; 	//定时时间及时间步长
	ladrc->r = ladrc_parameter[label][1]; 	//跟踪速度参数
	ladrc->wc = ladrc_parameter[label][2]; 	//观测器带宽
	ladrc->w0 = ladrc_parameter[label][3]; 	//状态误差反馈率带宽
	ladrc->b0 = ladrc_parameter[label][4]; 	//系统参数      
}

/* sign函数 */
float sign(float input)
{
	float output = 0;
	// if(input>0){
	// 	output = 1;
	// }
	// else if(input==0){
	// 	output = 0;
	// }
	// else{
	// 	output = -1;
	// }

    output = input > 0 ? 1 :( input == 0 ? 0 : -1 );
	return output;
}

/* 最速综合函数fhan */
float fhan(float x1,float x2,float r0,float h0)  
{
	float d = 0 ,a0 = 0, y = 0,a1 = 0,a2 = 0,sy = 0,a = 0,sa = 0;
	float fhan = 0;   
	d = r0*h0*h0;
	a0 = h0*x2;
	y = x1 + a0;
	a1 = sqrt(d*(d + 8*fabs(y)));
	a2 = a0 + sign(y)*(a1-d)*0.5F;
	sy = (sign(y+d) - sign(y-d))*0.5F;
	a = (a0+y-a2)*sy + a2;
	sa = ( sign(a+d) - sign(a-d) )*0.5F;
	fhan = -r0*(a/d - sign(a))*sa - r0*sign(a);
	return fhan;	
}

/**
  * @brief   复位
  * @param   *ladrc：参数
  */
void LADRC_RESET(LADRC_CTRL *ladrc)
{
	ladrc->z1 = 0; //定时时间及时间步长
	ladrc->z2 = 0; //跟踪速度参数
	ladrc->z3 = 0; //观测器带宽
}

/**
  * @brief   微分跟踪器：防止目标突变安排过渡过程，平滑及减少闭环传递函数的超调
  * @param   *ladrc：参数
  * @param   expect：期望值
  * @param   out：跟踪信号v1和微分信号v2
  */
void ADRC_TD(LADRC_CTRL *ladrc,float expect)
{
	float fh = 0;
	fh = fhan(ladrc->v1 - expect, ladrc->v2 , ladrc->r, ladrc->h);
	ladrc->v1 = ladrc->v1 + (ladrc->v2*ladrc->h);
	ladrc->v2 = ladrc->v2 + fh*ladrc->h;
}

/**
  * @brief   扩张状态观测器
  * @param   *ladrc：参数
  * @param   feedback：反馈值
  * @param   out：z1输出系统输出的观测值，z2输出微分值，z3输出总扰动
  */
void ADRC_ESO(LADRC_CTRL *ladrc,float feedback)
{
	float beta_01,beta_02,beta_03;  /* 反馈增益1、2、3 */
	float error;
	beta_01 = 3*ladrc->w0;
	beta_02 = 3*ladrc->w0*ladrc->w0;
	beta_03 = ladrc->w0 * ladrc->w0 * ladrc->w0;
	
	error = ladrc->z1 - feedback;																							/* z1：跟随反馈feedback */
	ladrc->z1 += ladrc->h*(ladrc->z2 - beta_01*error);
    ladrc->z2 += ladrc->h*(ladrc->z3 - beta_02*error + ladrc->b0*ladrc->u);   /* z2：跟随反馈feedback的微分 */
	ladrc->z3 += -beta_03*error*ladrc->h;   /* z3：系统扩张的一个状态，观测的是系统的总扰动，自控扰灵魂所在 */
}

/**
  * @brief   线性误差反馈控制率：线性PD控制器，补偿扰动误差
  * @param   *ladrc：参数
  */
void ADRC_LSEF(LADRC_CTRL *ladrc)
{
	float Kp,Kd,e1,e2,u0;
	
	Kp = ladrc->wc*ladrc->wc;
	Kd = 2*ladrc->wc;
	
	e1 = ladrc->v1 - ladrc->z1;
	e2 = ladrc->v2 - ladrc->z2;
	u0 = Kp*e1 + Kd*e2;
	ladrc->u = (u0-ladrc->z3) / ladrc->b0;	/* 系统的最终输出 */
}

/**
  * @brief   adrc控制
  * @param   *ladrc：参数
  */
void adrc_controller(LADRC_CTRL *ladrc,float expect , float feedback)  	
{
	ADRC_TD(ladrc,expect);
	ADRC_ESO(ladrc,feedback);
	ADRC_LSEF(ladrc);
}

